import serial
import re
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from collections import deque
import sys

# ------------------ Serial port configuration ------------------
SERIAL_PORT = '/dev/tty.usbmodem11303'  # 根据你的实际情况修改
BAUD_RATE = 115200

try:
    ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
    print(f"Connected to {SERIAL_PORT} at {BAUD_RATE} baud.")
except Exception as e:
    print("Error opening serial port:", e)
    sys.exit(1)

# ------------------ regex ------------------
pattern_time    = re.compile(r"\[(\d+)\s+ms\]")  # timestamp
pattern_accel   = re.compile(r"Accel:\s*([-+]?\d+\.\d+),\s*([-+]?\d+\.\d+),\s*([-+]?\d+\.\d+)\s*\|\s*FIFO Left:\s*(\d+)")
pattern_temp    = re.compile(r"Temp:\s*([-+]?\d+\.\d+)\s*\|\s*FIFO Left:\s*(\d+)")
pattern_humid   = re.compile(r"Humidity:\s*([-+]?\d+\.\d+)\s*\|\s*FIFO Left:\s*(\d+)")
pattern_press   = re.compile(r"Pressure:\s*([-+]?\d+\.\d+)\s*\|\s*FIFO Left:\s*(\d+)")
pattern_gyro    = re.compile(r"Gyro:\s*([-+]?\d+\.\d+),\s*([-+]?\d+\.\d+),\s*([-+]?\d+\.\d+)\s*\|\s*FIFO Left:\s*(\d+)")
pattern_magneto = re.compile(r"Magneto:\s*([-+]?\d+),\s*([-+]?\d+),\s*([-+]?\d+)\s*\|\s*FIFO Left:\s*(\d+)")
pattern_emergency_tilt = re.compile(r"^Emergency:\s*\[(\d+)\s+ms\]\s*Tilt detected!")
# 新增用于匹配 Activity Detected 消息的正则
pattern_activity = re.compile(r"\[\d+\s+ms\]\s*Activity Detected:.*")

# ------------------ Global variable for Activity ------------------
latest_activity = ""  # 用于保存最新的 Activity Detected 消息

# ------------------ Define data queues ------------------
WINDOW_SIZE = 100

time_accel   = deque(maxlen=WINDOW_SIZE)
accel_x      = deque(maxlen=WINDOW_SIZE)
accel_y      = deque(maxlen=WINDOW_SIZE)
accel_z      = deque(maxlen=WINDOW_SIZE)
fifo_accel   = deque(maxlen=WINDOW_SIZE)

time_temp    = deque(maxlen=WINDOW_SIZE)
temp_values  = deque(maxlen=WINDOW_SIZE)
fifo_temp    = deque(maxlen=WINDOW_SIZE)

time_humid   = deque(maxlen=WINDOW_SIZE)
humid_values = deque(maxlen=WINDOW_SIZE)
fifo_humid   = deque(maxlen=WINDOW_SIZE)

time_press   = deque(maxlen=WINDOW_SIZE)
press_values = deque(maxlen=WINDOW_SIZE)
fifo_press   = deque(maxlen=WINDOW_SIZE)

time_gyro    = deque(maxlen=WINDOW_SIZE)
gyro_x       = deque(maxlen=WINDOW_SIZE)
gyro_y       = deque(maxlen=WINDOW_SIZE)
gyro_z       = deque(maxlen=WINDOW_SIZE)
fifo_gyro    = deque(maxlen=WINDOW_SIZE)

time_magneto = deque(maxlen=WINDOW_SIZE)
magneto_x    = deque(maxlen=WINDOW_SIZE)
magneto_y    = deque(maxlen=WINDOW_SIZE)
magneto_z    = deque(maxlen=WINDOW_SIZE)
fifo_magneto = deque(maxlen=WINDOW_SIZE)

# ------------------ for Tilt Detection ------------------
tilt_event_time   = None          
tilt_display_start = None         
tilt_delay_list   = []            # （ms）
TILT_DISPLAY_DURATION = 2000      

# ------------------ Matplotlib (Figure 1: Sensor signals) ------------------
fig, ax = plt.subplots(4, 1, figsize=(10, 10))

# Subplot 0: Accelerometer
ax[0].set_title("Accelerometer (m/s^2)")
line_accel_x, = ax[0].plot([], [], label="Accel X")
line_accel_y, = ax[0].plot([], [], label="Accel Y")
line_accel_z, = ax[0].plot([], [], label="Accel Z")
ax[0].legend()
ax[0].grid(True)

# Subplot 1: Gyroscope
ax[1].set_title("Gyroscope (dps)")
line_gyro_x, = ax[1].plot([], [], label="Gyro X")
line_gyro_y, = ax[1].plot([], [], label="Gyro Y")
line_gyro_z, = ax[1].plot([], [], label="Gyro Z")
ax[1].legend()
ax[1].grid(True)

# Subplot 2: Temperature / Humidity / Pressure
ax[2].set_title("Temp / Humidity / Pressure (100hPa)")
line_temp, = ax[2].plot([], [], label="Temp (°C)")
line_humid, = ax[2].plot([], [], label="Humidity (%)")
line_press, = ax[2].plot([], [], label="Pressure (100 hPa)")
ax[2].legend()
ax[2].grid(True)

# Subplot 3: Magnetometer
ax[3].set_title("Magnetometer")
line_magneto_x, = ax[3].plot([], [], label="Magneto X")
line_magneto_y, = ax[3].plot([], [], label="Magneto Y")
line_magneto_z, = ax[3].plot([], [], label="Magneto Z")
ax[3].legend()
ax[3].grid(True)

plt.tight_layout()

# Tilt annotation（显示在 Accelerometer 图上）
tilt_annotation = ax[0].text(0.05, 0.9, "", transform=ax[0].transAxes,
                               fontsize=12, color='red',
                               bbox=dict(facecolor='yellow', alpha=0.5))
# 新增 Activity annotation（显示最新的 Activity Detected 消息，放在 Accelerometer 图中）
activity_annotation = ax[0].text(0.05, 0.75, "", transform=ax[0].transAxes,
                                 fontsize=12, color='blue',
                                 bbox=dict(facecolor='white', alpha=0.5))

# ------------------ FIFO lists (for later bar plot) ------------------
fifo_accel_list   = []
fifo_temp_list    = []
fifo_humid_list   = []
fifo_press_list   = []
fifo_gyro_list    = []
fifo_magneto_list = []

# ------------------ update plot ------------------
def update_plot(frame):
    global tilt_event_time, tilt_display_start, tilt_delay_list, latest_activity
    # 读取串口数据
    while ser.in_waiting:
        line = ser.readline().decode('utf-8', errors='replace').strip()
        if not line:
            continue

        # Tilt Detection
        m = pattern_emergency_tilt.match(line)
        if m:
            event_time = float(m.group(1))
            tilt_event_time   = event_time  
            tilt_display_start = event_time  
            print(f"[EMERGENCY] Tilt detected at {event_time} ms!")
            continue

        # 如果是 Activity Detected 消息，则更新全局变量并跳过后续处理
        if pattern_activity.match(line):
            latest_activity = line
            continue

        time_match = pattern_time.search(line)
        if time_match:
            t_val = float(time_match.group(1))
        else:
            continue

        # Process Accelerometer
        if match := pattern_accel.search(line):
            x, y, z, fifo_val = match.groups()
            x, y, z = map(float, (x, y, z))
            fifo_val = int(fifo_val)
            time_accel.append(t_val)
            accel_x.append(x)
            accel_y.append(y)
            accel_z.append(z)
            fifo_accel.append(fifo_val)
            # 同时保存到 FIFO 列表（用于后续 FIFO 平均图）
            fifo_accel_list.append(fifo_val)

            # 如果存在 Tilt 事件，计算延迟
            if tilt_event_time is not None and t_val > tilt_event_time:
                delay = t_val - tilt_event_time
                tilt_delay_list.append(delay)
                print(f"Measured tilt response delay: {delay} ms")
                tilt_event_time = None  # 每个 Tilt 只计算一次延迟

        # Process Temperature
        if match := pattern_temp.search(line):
            t_str, fifo_val = match.groups()
            t = (float(t_str) -32.7)/1.8  # 华氏度转摄氏度
            fifo_val = int(fifo_val)
            time_temp.append(t_val)
            temp_values.append(t)
            fifo_temp.append(fifo_val)
            fifo_temp_list.append(fifo_val)
        # Process Humidity
        if match := pattern_humid.search(line):
            h_str, fifo_val = match.groups()
            h = float(h_str)
            fifo_val = int(fifo_val)
            time_humid.append(t_val)
            humid_values.append(h)
            fifo_humid.append(fifo_val)
            fifo_humid_list.append(fifo_val)
        # Process Pressure
        if match := pattern_press.search(line):
            p_str, fifo_val = match.groups()
            p = float(p_str) / 100.0
            fifo_val = int(fifo_val)
            time_press.append(t_val)
            press_values.append(p)
            fifo_press.append(fifo_val)
            fifo_press_list.append(fifo_val)
        # Process Gyroscope
        if match := pattern_gyro.search(line):
            gx, gy, gz, fifo_val = match.groups()
            gx, gy, gz = map(float, (gx, gy, gz))
            fifo_val = int(fifo_val)
            time_gyro.append(t_val)
            gyro_x.append(gx)
            gyro_y.append(gy)
            gyro_z.append(gz)
            fifo_gyro.append(fifo_val)
            fifo_gyro_list.append(fifo_val)
        # Process Magnetometer
        if match := pattern_magneto.search(line):
            mx, my, mz, fifo_val = match.groups()
            mx, my, mz = map(int, (mx, my, mz))
            fifo_val = int(fifo_val)
            time_magneto.append(t_val)
            magneto_x.append(mx)
            magneto_y.append(my)
            magneto_z.append(mz)
            fifo_magneto.append(fifo_val)
            fifo_magneto_list.append(fifo_val)

    # 更新 Accelerometer 曲线
    if len(time_accel) > 0:
        line_accel_x.set_data(list(time_accel), list(accel_x))
        line_accel_y.set_data(list(time_accel), list(accel_y))
        line_accel_z.set_data(list(time_accel), list(accel_z))
        ax[0].set_xlim(min(time_accel), max(time_accel))
    ax[0].set_ylim(-20, 20)

    # 更新 Gyroscope 曲线
    if len(time_gyro) > 0:
        line_gyro_x.set_data(list(time_gyro), list(gyro_x))
        line_gyro_y.set_data(list(time_gyro), list(gyro_y))
        line_gyro_z.set_data(list(time_gyro), list(gyro_z))
        ax[1].set_xlim(min(time_gyro), max(time_gyro))
    ax[1].set_ylim(-500, 500)

    # 更新 Temperature / Humidity / Pressure 曲线
    if len(time_temp) > 0:
        line_temp.set_data(list(time_temp), list(temp_values))
        ax[2].set_xlim(min(time_temp), max(time_temp))
    if len(time_humid) > 0:
        line_humid.set_data(list(time_humid), list(humid_values))
    if len(time_press) > 0:
        line_press.set_data(list(time_press), list(press_values))
    ax[2].set_ylim(-10, 120)

    # 更新 Magnetometer 曲线
    if len(time_magneto) > 0:
        line_magneto_x.set_data(list(time_magneto), list(magneto_x))
        line_magneto_y.set_data(list(time_magneto), list(magneto_y))
        line_magneto_z.set_data(list(time_magneto), list(magneto_z))
        ax[3].set_xlim(min(time_magneto), max(time_magneto))
    ax[3].set_ylim(-2500, 200)

    # ---------------- Tilt Annotation ----------------
    current_time = time_accel[-1] if len(time_accel) > 0 else 0
    if tilt_display_start is not None and (current_time - tilt_display_start) < TILT_DISPLAY_DURATION:
        tilt_annotation.set_text("TILT detected!")
    else:
        tilt_annotation.set_text("")
        tilt_display_start = None

    # ---------------- Activity Annotation ----------------
    # 实时更新最新的 Activity Detected 消息（显示在 Accelerometer 图中）
    activity_annotation.set_text(latest_activity)

    # 返回所有需要刷新的对象（增加 tilt_annotation 和 activity_annotation）
    return (line_accel_x, line_accel_y, line_accel_z,
            line_gyro_x, line_gyro_y, line_gyro_z,
            line_temp, line_humid, line_press,
            line_magneto_x, line_magneto_y, line_magneto_z,
            tilt_annotation, activity_annotation)

# ------------------ Start Animation ------------------
ani = animation.FuncAnimation(fig, update_plot, interval=100, blit=False)
plt.show()

# 计算 FIFO 平均值，用于第二幅图展示
def average_fifo(fifo_list):
    if fifo_list:
        return sum(fifo_list) / len(fifo_list)
    return 0

avg_fifo_accel   = average_fifo(fifo_accel)
avg_fifo_temp    = average_fifo(fifo_temp)
avg_fifo_humid   = average_fifo(fifo_humid)
avg_fifo_press   = average_fifo(fifo_press)
avg_fifo_gyro    = average_fifo(fifo_gyro)
avg_fifo_magneto = average_fifo(fifo_magneto)

# ------------------ Visualize FIFO Data (Figure 2) ------------------
fig2, ax2 = plt.subplots(figsize=(10,6))
sensors = ['Accel', 'Temp', 'Humid', 'Press', 'Gyro', 'Magneto']
fifo_avgs = [avg_fifo_accel, avg_fifo_temp, avg_fifo_humid, avg_fifo_press, avg_fifo_gyro, avg_fifo_magneto]

bars = ax2.bar(sensors, fifo_avgs, color='skyblue')
ax2.set_ylabel("Average FIFO Left")
ax2.set_title("Average FIFO Left per Sensor")

if tilt_delay_list:
    avg_delay = sum(tilt_delay_list) / len(tilt_delay_list)
    ax2.text(0.5, 0.9, f"Average Tilt Response Delay: {avg_delay:.2f} ms", 
             transform=ax2.transAxes, ha='center', va='center', fontsize=14, color='red')
else:
    ax2.text(0.5, 0.9, "No Tilt events recorded", 
             transform=ax2.transAxes, ha='center', va='center', fontsize=14, color='red')
plt.show()

# ------------------ Close UART ------------------
ser.close()
